
#include <iostream>
#include <sstream>
#include <vector>
#include <pthread.h>

#include "geomTypes.h"

typedef float                                   Number;
typedef Point<3, Number>                        Point3;
typedef Vector<3, Number>                       Vector3;

struct Segment3 {
   const Point3 operator[](int i) const { if (i==0) return s; else return t; }
   Point3 source() const { return s; };
   Point3 target() const { return t; };
   Point3 s;
   Point3 t;
};

#include "../matrix.h"
typedef MatrixT<Number>                         Matrix;

#include "IOoffs.h"
#include "../core/tconstraints.h"
#include "../core/mlsDeformer.h"
#include "mlsRaw.h"
#include "mlsAffine.h"
#include "mlsDeformerFS.h"
//#include "mlsComparison.h"
#include <GL/glut.h>

using namespace std;

int winw = 600, winh = 600; // window size
float rotate_x = 0.0, rotate_y = 0.0;
int mouse_old_x, mouse_old_y;
int mouse_buttons = 0;

/// time info
float      cfps;
time_t     start, end;
unsigned   frame;

/// Info variables
void* font=GLUT_BITMAP_TIMES_ROMAN_24;
void* fontsmall=GLUT_BITMAP_HELVETICA_12;
string snvertices("Vertices: ");
string snfaces("Faces: ");
string snconstraints("Faces: ");
string fps("FPS: ");

void idle();
/// Renders a string
void renderBitmapString(float x, float y, float z, void *font, const char *string);
/// Renders info about the process
void renderInfo();

/// MLS data
const int mesh_width = 256;
const int mesh_height = 256;
int nvertices;
int nfaces;
vector<Point3>    mesh_pos;
vector<Point3>    mesh_pos_Ini;
vector<Vector3>   mesh_normals;
vector<Vector3>   mesh_normals_Ini;
vector<unsigned>  mesh_faces;

typedef TConstraints<Point3>        PConstraints;
PConstraints P, Q;
/*
typedef MlsDeformer<PConstraints>   MLSDeformer;
MLSDeformer deformer;
*/
MlsDeformerFS *deformerfs=0;

bool flag = true;
bool flagCtrlPoints = true;
float T[3]={0,0,0};
bool crescente = true;
GLuint vbo; /// vertex buffer object id
GLuint nbo; /// normal buffer object id
GLuint ebo; /// element buffer object id

/*
void drawBBDefWires() {
   
   const Point3 &q0 = Q.getConstraint(0);
   const Point3 &q1 = Q.getConstraint(1);
   const Point3 &q2 = Q.getConstraint(2);
   const Point3 &q3 = Q.getConstraint(3);
   const Point3 &q4 = Q.getConstraint(4);
   const Point3 &q5 = Q.getConstraint(5);
   const Point3 &q6 = Q.getConstraint(6);
   const Point3 &q7 = Q.getConstraint(7);
   glBegin(GL_LINE_LOOP);
      glVertex3f(q0[0], q0[1], q0[2]);
      glVertex3f(q1[0], q1[1], q1[2]);
      glVertex3f(q2[0], q2[1], q2[2]);
      glVertex3f(q3[0], q3[1], q3[2]);
   glEnd();
   glBegin(GL_LINE_LOOP);
      glVertex3f(q4[0], q4[1], q4[2]);
      glVertex3f(q5[0], q5[1], q5[2]);
      glVertex3f(q6[0], q6[1], q6[2]);
      glVertex3f(q7[0], q7[1], q7[2]);
   glEnd();
   glBegin(GL_LINES);
      glVertex3f(q0[0], q0[1], q0[2]);
      glVertex3f(q4[0], q4[1], q4[2]);
      glVertex3f(q1[0], q1[1], q1[2]);
      glVertex3f(q5[0], q5[1], q5[2]);
      glVertex3f(q2[0], q2[1], q2[2]);
      glVertex3f(q6[0], q6[1], q6[2]);
      glVertex3f(q3[0], q3[1], q3[2]);
      glVertex3f(q7[0], q7[1], q7[2]);
   glEnd();
}
*/

/// OpenGL display function
void display() {
   
   glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
   
   glMatrixMode(GL_PROJECTION);
   glLoadIdentity();
   glOrtho(-1.2,1.2,-1.2,1.2,-4,4);
   //~ gluPerspective(60.0, (GLfloat) winw/(GLfloat) winh, 1.0, 20.0);
   //~ gluLookAt (0.0, 0.0, 3.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0);
   
   glMatrixMode(GL_MODELVIEW);
   glLoadIdentity();
   glViewport(0, 0, (GLsizei) winw, (GLsizei) winh);
   
   //~ glRotatef(3, 1.0, 0.0, 0.0);
   //~ glRotatef(5, 0.0, 1.0, 0.0);
   glRotatef(rotate_x, 1.0, 0.0, 0.0);
   glRotatef(rotate_y, 0.0, 1.0, 0.0);
   
   glEnable(GL_LIGHTING);
      /// vbo
      glBindBuffer(GL_ARRAY_BUFFER, vbo); // set vertices from the vbo
      unsigned int sizev = nvertices*3*sizeof(float);
      glBufferSubData(GL_ARRAY_BUFFER, 0, sizev, &mesh_pos[0]);
      glVertexPointer(3, GL_FLOAT, 0, 0);
      glBindBuffer(GL_ARRAY_BUFFER, nbo);
      glBufferSubData(GL_ARRAY_BUFFER, 0, sizev, &mesh_normals[0]);
      glNormalPointer(GL_FLOAT, 0, 0);
         glEnableClientState(GL_VERTEX_ARRAY);
         glEnableClientState(GL_NORMAL_ARRAY);
         glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, ebo);
         glDrawElements(GL_TRIANGLES, 3*nfaces, GL_UNSIGNED_INT, 0);
         glDisableClientState(GL_VERTEX_ARRAY);
         glDisableClientState(GL_NORMAL_ARRAY);
      glBindBuffer(GL_ARRAY_BUFFER, 0);
      glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
      
   /// draw constraints
   if (flagCtrlPoints) {
      glPushAttrib(GL_COLOR_BUFFER_BIT | GL_LIGHTING_BIT | GL_POINT_BIT);
      glDisable(GL_LIGHTING);
      glEnable(GL_POINT_SMOOTH);
      glEnable(GL_BLEND);
      glColor4f(0,0,0,0);
      for (unsigned int i=0; i<Q.size(); ++i) {
         const Point3 &p = Q.getConstraint(i);
         glPointSize(20);
         //~ if (p[2]>0.5)  glPointSize(20);
         //~ if (p[2]<0.5)  glPointSize(15);
         //~ else           glPointSize(18);
         glBegin(GL_POINTS);
         glVertex3f(p[0], p[1], p[2]);
         glEnd();
      }
      
      //~ glColor4f(0.3,0.3,0.3,1.0);
      //~ drawBBDefWires();
      
      glPopAttrib();
   }
   
   /// FPS info
   //~ frame++;
   //~ std::time(&end);
   //~ double ddifftime = std::difftime(end, start);
   //~ if (ddifftime > 1) {
      //~ cfps = frame/ddifftime;
      //~ start = end;
      //~ frame = 0;
      //~ ostringstream s1;
      //~ s1<<cfps;
      //~ fps = "FPS: " + s1.str();
   //~ }
   //~ glPushMatrix();
   //~ glLoadIdentity();
   //~ renderInfo();
   //~ glPopMatrix();
   
   glutSwapBuffers();
}

/*
/// Defines a struct for threads processing
struct channel_t {
   uint   tid;       // channels identifier
   uint   ibegin;    // index begin
   uint   iend;      // index end
};

void *mls_t(void *ptr) {
   
   channel_t *ch = (channel_t *)ptr;
   float p[3], pd[3];
   for (unsigned int i=ch->ibegin; i<ch->iend; ++i) {
      p[0] = mesh_pos_Ini[i][0];
      p[1] = mesh_pos_Ini[i][1];
      p[2] = mesh_pos_Ini[i][2];
      //mlsEval(P.positions, Q.positions, p, pd);
      mesh_pos[i] = Point3(pd[0], pd[1], pd[2]);
   }
   
   pthread_exit(NULL);
}

const unsigned int NUMTHREADS=2;
pthread_t threads[NUMTHREADS];
channel_t channels[NUMTHREADS];

void mlsThreads() {
   
   uint blocksize = (unsigned)(ceil(nvertices/NUMTHREADS));
   uint ini = 0;
   uint end = blocksize;
   for (unsigned j=0; j<NUMTHREADS; ++j) {
      channels[j].tid = j;
      channels[j].ibegin = ini;
      channels[j].iend = (j==NUMTHREADS-1)? nvertices:end;
      
      pthread_create(&(threads[j]), NULL, mls_t, &(channels[j]));
      ini = end;
      end += blocksize;
   }
   for(unsigned j=0; j<NUMTHREADS; ++j)
      pthread_join(threads[j], NULL);
}
*/

vector<int> handlesids; // ids of control points handles
Number MT[4][4];
float angle=0.0;

void idle() {
   
   //~ Q.setConstraint(0, P.getConstraint(0) + Vector3 (0.5,1.0,0.0)); // girl
   //~ Q.setConstraint(0, P.getConstraint(0) + Vector3 (0.4,-0.7,0.0)); // dino
   //~ Q.setConstraint(1, P.getConstraint(1) + Vector3 (0.4,-0.7,0.0)); // dino
   
   // comparisons ok
   //~ Q.setConstraint(0, P.getConstraint(0) + Vector3 (0.9,-0.5,0.0)); // girl
   //~ Q.setConstraint(0, P.getConstraint(0) + Vector3 (0.0,-0.7,0.0)); // dino
   //~ Q.setConstraint(1, P.getConstraint(1) + Vector3 (0.0,-0.7,0.0)); // dino
   //~ Q.setConstraint(0, P.getConstraint(0) + Vector3 (0.0,-0.8,0.0)); // horse
   //~ Q.setConstraint(0, P.getConstraint(0) + Vector3 (0.0,0.8,0.0)); // dragon
   //~ Q.setConstraint(1, P.getConstraint(1) + Vector3 (-0.4,0.0,0.0)); // dragon
   /// Raw version
   //~ float p[3], pd[3];
   //~ for (int i=0; i<nvertices; ++i) {
      //~ p[0] = mesh_pos_Ini[i][0];
      //~ p[1] = mesh_pos_Ini[i][1];
      //~ p[2] = mesh_pos_Ini[i][2];
      //~ mlsEval(P.positions, Q.positions, p, pd);
      //~ mesh_pos[i] = Point3(pd[0], pd[1], pd[2]);
   //~ }
   //~ deformerfs->execute(mesh_pos, Q);
   //~ saveOFFfile("dragon-gpu-fs.off");
   //~ exit(0);
   
   /// visual version
   //~ if (crescente) T[1] = T[1] + 0.1;
   //~ else           T[1] = T[1] - 0.1;
   //~ if (T[1] > 0.5)       crescente = false;
   //~ else if (T[1] < -0.5) crescente = true;
   
   //~ const Point3 &cp = P.getConstraint(8);
   //~ Q.setConstraint(8,Point3(cp[0]+T[0], cp[1]+T[1], cp[2]+T[2]));
   
   if (crescente) angle += 10;
   else           angle -= 10;
   if (angle > 270)       crescente = false;
   else if (angle < 0.0) crescente = true;
   
   float a = angle*3.1416/180.0; // angle
   
   MT[0][0]=cos(a);  MT[0][1]=0.0;  MT[0][2]=sin(a); MT[0][3]=0.0;
   MT[1][0]=0.0;     MT[1][1]=1.0;  MT[1][2]=0.0;    MT[1][3]=0.0;
   MT[2][0]=-sin(a); MT[2][1]=0.0;  MT[2][2]=cos(a); MT[2][3]=0.0;
   MT[3][0]=0.0;     MT[3][1]=0.0;  MT[3][2]=0.0;    MT[3][3]=1.0;
   
   if (flag) {
      for (unsigned int i=0; i<P.size()/2; ++i) {
         const Point3 &p = P.getConstraint(handlesids[i]);
         Number x = p[0]*MT[0][0] + p[1]*MT[0][1] + p[2]*MT[0][2] + MT[0][3];
         Number y = p[0]*MT[1][0] + p[1]*MT[1][1] + p[2]*MT[1][2] + MT[1][3];
         Number z = p[0]*MT[2][0] + p[1]*MT[2][1] + p[2]*MT[2][2] + MT[2][3];
         Q.setConstraint(handlesids[i], Point3(x,y,z));
      }
      /// Raw version
      float p[3], pd[3]; float n[3], nd[3];
      for (int i=0; i<nvertices; ++i) {
         p[0] = mesh_pos_Ini[i][0]; p[1] = mesh_pos_Ini[i][1]; p[2] = mesh_pos_Ini[i][2];
         n[0] = mesh_normals_Ini[i][0]; n[1] = mesh_normals_Ini[i][1]; n[2] = mesh_normals_Ini[i][2];
         mlsEval(P.positions, Q.positions, p, pd, n, nd);
         //~ mlsAffineEval(P.positions, Q.positions, p, pd);
         if (isnan(pd[0]) or isnan(pd[1]) or isnan(pd[2])) {
            mesh_pos[i] = Point3(p[0], p[1], p[2]);
            mesh_normals[i] = Vector3(n[0], n[1], n[2]);
         }
         else {
            mesh_pos[i] = Point3(pd[0], pd[1], pd[2]);
            mesh_normals[i] = Vector3(nd[0], nd[1], nd[2]);
         }
      }
      //~ saveOFFfile("saida.off");
   }
   
   /*
   /// Horn version: Orthonormal matrices
   float p[3], pd[3];
   for (int i=0; i<nvertices; ++i) {
      p[0] = mesh_pos_Ini[i][0];
      p[1] = mesh_pos_Ini[i][1];
      p[2] = mesh_pos_Ini[i][2];
      mlsEval_OM_Horn(P.positions, Q.positions, p, pd);
      //mlsEval_SVD(P.positions, Q.positions, p, pd);
      //mlsEval_Quat(P.positions, Q.positions, p, pd);
      mesh_pos[i] = Point3(pd[0], pd[1], pd[2]);
   }
   */
   
   /// OO version
   //~ for (int i=0; i<nvertices; ++i) {
      //~ mesh_pos[i] = deformer.eval(mesh_pos_Ini[i]);
   //~ }
   
   /// multicore version
   //~ mlsThreads();
   
   /// FS version
   //~ deformerfs->execute(mesh_pos, Q);
   
   glutPostRedisplay();
}

void buildMesh(string filename) {
   
   string extensao =filename.substr(filename.size()-3,filename.size());
   if (extensao == "off") {
      readOFFfile(filename.c_str());
      mesh_pos_Ini = mesh_pos;
      mesh_normals_Ini = mesh_normals;
   } else {
      cout<<"The input is not an OFF file"<<endl;
      assert(false);
   }
}

void createControlPoints(string filename) {
   
   string extensao1 = filename.substr(filename.size()-2,filename.size());
   string extensao2 = filename.substr(filename.size()-3,filename.size());
   if (extensao1 == "cp") {
      ifstream file(filename.c_str());
      int id;
      while(not file.eof()) {
         file>>id;
         P.push_back(mesh_pos_Ini[id]);
      }
      // insert the non-fixed handles
      for (unsigned int i=0; i<P.size()/2; ++i)
         handlesids.push_back(i);
      
      Q = P;
   } else if (extensao2 == "cpp") {
      ifstream file(filename.c_str());
      float x, y, z;
      while(not file.eof()) {
         file>>x>>y>>z;
         P.push_back(Point3(x,y,z));
      }
      // insert the non-fixed handles
      for (unsigned int i=0; i<P.size()/2; ++i)
         handlesids.push_back(i);
      
      Q = P;
   } else {
      cout<<"The input is not a CP file"<<endl;
      assert(false);
   }
}

/// Renders a string
void renderBitmapString(float x, float y, float z, void *font, const char *string) {
   
   const char *c;
   glRasterPos3f(x, y,z);
   for (c=string; *c != '\0'; c++) {
      glutBitmapCharacter(font, *c);
   }
}

/// Renders info about the process
void renderInfo() {
   
   glDisable(GL_DEPTH_TEST);
   glColor4f(0,0,0,1);
   renderBitmapString(-1.0,1.0,0.0,(void *)font, fps.c_str());
   renderBitmapString(-0.9,0.90,0,(void *)fontsmall, snvertices.c_str());
   renderBitmapString(-0.9,0.85,0,(void *)fontsmall, snfaces.c_str());
   renderBitmapString(-0.9,0.80,0,(void *)fontsmall, snconstraints.c_str());
   glEnable(GL_DEPTH_TEST);
}

/// OpenGL initializations
void init() {
   
   glClearColor(1,1,1,1);
   glColor4f(1,1,0,1);
   
   std::time(&start);
   frame = 0;
   
   /// ///////////////////////////////////
   glEnable(GL_NORMALIZE);
   glEnable(GL_DEPTH_TEST);
   glEnable(GL_DITHER);
   glEnable(GL_LIGHTING);
   glLightModeli(GL_LIGHT_MODEL_TWO_SIDE, GL_FALSE);
   
   GLfloat mat_A[] = {0.5, 0.5, 0.5, 1.0};
   GLfloat mat_D[] = {0.0, 0.4, 0.6, 1.0};
   GLfloat mat_S[] = {0.4, 0.4, 0.4, 1.0};
   GLfloat shine[] = {128.0};
   glMaterialfv(GL_FRONT, GL_AMBIENT,   mat_A);
   glMaterialfv(GL_FRONT, GL_DIFFUSE,   mat_D);
   glMaterialfv(GL_FRONT, GL_SPECULAR,  mat_S);
   glMaterialfv(GL_FRONT, GL_SHININESS, shine);
   
   GLfloat pos_L0[] = { 10.0, 10.0, 2.0, 0.0};
   GLfloat pos_L1[] = {-10.0, 10.0, 2.0, 0.0};
   GLfloat pos_L2[] = { 0.0, 0.0, 10.0, 0.0};
   GLfloat col_2[]  = { 1.0, 1.0, 1.0, 1.0};
   glEnable(GL_LIGHT0);
   glEnable(GL_LIGHT1);
   glEnable(GL_LIGHT2);
   glLightfv(GL_LIGHT0, GL_POSITION, pos_L0);
   glLightfv(GL_LIGHT1, GL_POSITION, pos_L1);
   glLightfv(GL_LIGHT2, GL_POSITION, pos_L2);
   glLightfv(GL_LIGHT2, GL_DIFFUSE,  col_2);
   glLightfv(GL_LIGHT2, GL_SPECULAR, col_2);
   
   /// VBO
   // create vertex buffer object
   glGenBuffers(1, &vbo);
   glEnableClientState(GL_VERTEX_ARRAY);
   glBindBuffer(GL_ARRAY_BUFFER, vbo);
   unsigned int sizev = nvertices*3*sizeof(float);
   glBufferData(GL_ARRAY_BUFFER, sizev, &mesh_pos[0], GL_DYNAMIC_DRAW); // initialize buffer object
   
   // create normals buffer object
   glGenBuffers(1, &nbo);
   glEnableClientState(GL_NORMAL_ARRAY);
   glBindBuffer(GL_ARRAY_BUFFER, nbo);
   unsigned int sizen = nvertices*3*sizeof(float);
   glBufferData(GL_ARRAY_BUFFER, sizen, &mesh_normals[0], GL_DYNAMIC_DRAW);
   
   // create indices buffer object
   glGenBuffers(1, &ebo);
   glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, ebo);
   glBufferData(GL_ELEMENT_ARRAY_BUFFER, nfaces*3*(sizeof(unsigned int)), &mesh_faces[0], GL_STATIC_DRAW);
   
   // reset the binding
   glBindBuffer(GL_ARRAY_BUFFER, 0);
   glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
   
   //deformer.setSourceConstraints(P);
   //deformer.setTargetConstraints(Q);
   //~ deformerfs = new MlsDeformerFS(mesh_pos_Ini, P);
}

/// OpenGL reshape funcion
void reshape (int w, int h) {
   
   glMatrixMode(GL_PROJECTION);
   glLoadIdentity();
   glOrtho(-1.2,1.2,-1.2,1.2,-4,4);
   
   glMatrixMode(GL_MODELVIEW);
   glLoadIdentity();
   winw = w; winh = h;
   glViewport (0, 0, (GLsizei) w, (GLsizei) h);
   glutPostRedisplay();
}

/// OpenGL key press function
void keypress(unsigned char key, int x, int y) {
   
   switch (key) {
      case 'c':
      case 'C': flagCtrlPoints = not flagCtrlPoints; break;
      case 27: {
         if (deformerfs==0) delete deformerfs;
         exit(0);
      }
   }
   glutPostRedisplay();
}

void mouseclick(int button, int state, int x, int y) {
   
   if (state == GLUT_DOWN) {
      mouse_buttons |= 1<<button;
   } else if (state == GLUT_UP) {
      mouse_buttons = 0;
   }
   
   mouse_old_x = x;
   mouse_old_y = y;
   glutPostRedisplay();
}

void mousemove(int x, int y) {
   
   float dx, dy;
   dx = x - mouse_old_x;
   dy = y - mouse_old_y;
   
   if (mouse_buttons & 1) {
      rotate_x += dy * 0.2;
      rotate_y += dx * 0.2;
   }
   
   mouse_old_x = x;
   mouse_old_y = y;
}

void printInfoAndExit() {
   
   cout<<endl;
   cout<<"Usage: mlsGlut filenameA.off filenameB.cp|filenameB.cpp"<<endl;
   exit(0);
}

/// Process the program's parameters
void input(int argc, char **argv) {
   
   int nargs = argc; // 
   if (nargs != 3) printInfoAndExit();
   
   buildMesh(argv[1]);
   createControlPoints(argv[2]);
   
   ostringstream osnvertices;
   osnvertices<<nvertices;
   snvertices = osnvertices.str();
   
   ostringstream osnfaces;
   osnfaces<<nfaces;
   snfaces = osnfaces.str();
   
   ostringstream osnconstraints;
   osnconstraints<<P.size();
   snconstraints = osnconstraints.str();
}

int main(int argc, char **argv) {
   
   // Process the input parameters
   input(argc, argv);
   
   // Glut setup
   glutInit(&argc, argv);
   glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH);
   glutInitWindowSize(winw, winh);
   glutInitWindowPosition(150, 0);
   glutCreateWindow("MLS deformation : times using several implementations");
   
   // OpenGL vars and data initialization
   init();
   
   // Callbacks registration
   glutDisplayFunc(display);
   glutReshapeFunc(reshape);
   glutMouseFunc(mouseclick);
   glutMotionFunc(mousemove);
   glutKeyboardFunc(keypress);
   glutIdleFunc(idle);
   glutMainLoop();
   
   return 0;
}
